#coding=utf-8
import numpy as np
#   Qt界面
from PySide2 import QtCore
from PySide2.QtCore import QTimer
from PySide2.QtWidgets import QApplication, QSlider,QLCDNumber,QRadioButton,QStatusBar,QLabel
from PySide2.QtUiTools import QUiLoader

from controller.params import Params


class JPanel:
    '''
        关节控制Panel
    '''
    def __init__(self,rootObject,jointNums=6):
        '''
        初始化
        :param jointNums:
        '''
        self.rootObject=rootObject
        self.jointNums = jointNums
        self.ui_Main = QUiLoader().load("gui/qt_ui_design/j_ctrl.ui")
        self.AddBars()
        #   绑定按钮
        self.ui_Main.initBtn.clicked.connect(self.SetBarValues)
        self.ui_Main.exeBtn.clicked.connect(self.SendCmd)
        self.ui_Main.releaseBtn.clicked.connect(self.ReleaseJs)
        self.ui_Main.aquireBtn.clicked.connect(self.SyncFromPhysical)
        self.ui_Main.calibBtn.clicked.connect(self.DumpCalibrate)
        self.syncT=QTimer()
        self.syncT.timeout.connect(self.SyncAuto)
        self.syncT.start(1.0/Params.controlParams["freq_med"])
    def AddBars(self, jointNums=6):
        self.jointNums = jointNums
        self.lBars = []
        self.rBars = []
        self.lDigits = []
        self.rDigits = []
        for i in range(self.jointNums):
            #   建立数组控制滑块
            sliderL = QSlider();
            sliderL.setMinimum(-209);
            sliderL.setMaximum(209);
            sliderL.setOrientation(QtCore.Qt.Orientation(1))
            sliderL.valueChanged.connect(self.UpdateDisplay)
            labelL = QLCDNumber()
            self.ui_Main.lctrlLayout.addWidget(labelL);
            self.ui_Main.lctrlLayout.addWidget(sliderL);
            self.lBars.append(sliderL);
            self.lDigits.append(labelL)

            labelR = QLCDNumber()
            sliderR = QSlider();
            sliderR.setMinimum(-209);
            sliderR.setMaximum(209);
            sliderR.setOrientation(QtCore.Qt.Orientation(1))
            sliderR.valueChanged.connect(self.UpdateDisplay)
            self.rBars.append(sliderR);
            self.rDigits.append(labelR)
            self.ui_Main.rctrlLayout.addWidget(labelR)
            self.ui_Main.rctrlLayout.addWidget(sliderR)
    def SyncAuto(self):
        '''
        自动同步
        :return:
        '''
        if self.ui_Main.autoSync.isChecked():
            self.SendCmd()
    def SendCmd(self):
        '''
            获取关节数据
        :return:
        '''
        l=[];r=[]
        for i in range(self.jointNums):
            l.append(self.lBars[i].value()/100.0+self.rootObject.lChain.jErrorStatic[i])
            r.append(self.rBars[i].value()/100.0+self.rootObject.rChain.jErrorStatic[i])
        h=self.ui_Main.headSlider.value()/100.0
        #同步数据
        self.rootObject.lChain.joints=np.array(l)-self.rootObject.lChain.jErrorStatic
        self.rootObject.rChain.joints=np.array(r)-self.rootObject.rChain.jErrorStatic
        self.rootObject.netBridge.SyncJoint(l,r,h,t=0)
    def SyncFromPhysical(self):
        '''
        获取实体机器人的各个关节数据
        :return:
        '''
        l,r,h=self.rootObject.netBridge.GetJointVals()
        l=np.array(l)-self.rootObject.lChain.jErrorStatic
        r=np.array(r)-self.rootObject.rChain.jErrorStatic
        self.rootObject.lChain.joints=l.copy()
        self.rootObject.rChain.joints=r.copy()
        self.SetBarValues(l,r,h)
    def DumpCalibrate(self):
        '''
        记录关节修正值
        :return:
        '''
        l = []
        r = []
        for i in range(self.jointNums):
            l.append(float(self.lBars[i].value() / 100.0+self.rootObject.lChain.jErrorStatic[i]))
            r.append(float(self.rBars[i].value() / 100.0+self.rootObject.rChain.jErrorStatic[i]))
        h = self.ui_Main.headSlider.value() / 100.0
        #   更新实时误差状态
        self.rootObject.lChain.jErrorStatic = np.array(l)
        self.rootObject.rChain.jErrorStatic = np.array(r)
        #   更新缓存误差状态
        calib={"err_static":{"l":[],"r":[],"h":0.0}}
        calib["err_static"]["l"]=l
        calib["err_static"]["r"] = r
        calib["err_static"]["h"] = h
        self.rootObject.DumpConfig(calib)
        self.SetBarValues()
    def SetBarValues(self,l=[0,0,0,0,0,0],r=[0,0,0,0,0,0],h=0.0):
        '''
            更新关节数据
        :param l:
        :param r:
        :return:
        '''
        for i in range(self.jointNums):
            self.rBars[i].setValue(int(r[i]*100))
            self.lBars[i].setValue(int(l[i] * 100))
        self.ui_Main.headSlider.setValue(int(h* 100))
    def ReleaseJs(self):
        self.rootObject.ReleaseJoints()
    def UpdateDisplay(self):
        for i in range(self.jointNums):
            self.lDigits[i].display(self.lBars[i].value()/100)
            self.rDigits[i].display(self.rBars[i].value()/100)